// Standard C++ header
#include <stdlib.h>
#include <iostream>
#include <vector>

// Qt Header
#include <QApplication>
#include <QGraphicsView>
#include <QGraphicsScene>
#include <QGraphicsPixmapItem>
#include <QGraphicsTextItem>

// OpenNI Header
#include <XnCppWrapper.h>

#include <QDebug>
#include "widget.h"

// namespace
using namespace std;

static const char *GESTURE_STR = "RaiseHand";





/* Class for control OpenNI device */
class COpenNI
{
public:
    COpenNI()
    {

    }

	/* Destructor */
	~COpenNI()
	{
        m_Context.StopGeneratingAll();
		m_Context.Release();
	}

	/* Initial OpenNI context and create nodes. */
	bool Initial()
	{
		// Initial OpenNI Context
		m_eResult = m_Context.Init();
		if( CheckError( "Context Initial failed" ) )
			return false;

        m_eResult = m_Context.SetGlobalMirror(true);
        if(CheckError( "Set Global Mirror Error" ))
            return false;

		// create image node
		m_eResult = m_Image.Create( m_Context );
		if( CheckError( "Create Image Generator Error" ) )
			return false;

		// create depth node
		m_eResult = m_Depth.Create( m_Context );
		if( CheckError( "Create Depth Generator Error" ) )
			return false;

		// set nodes
		m_eResult = m_Depth.GetAlternativeViewPointCap().SetViewPoint( m_Image );
		CheckError( "Can't set the alternative view point on depth generator" );


        // create gesture generator
        m_eResult = m_Gesture.Create( m_Context );
        if( CheckError( "Create Gestrue Generator Error" ) )
            return false;
        XnCallbackHandle hGestrue;
        m_Gesture.RegisterGestureCallbacks(GestureRecognized, NULL, NULL, hGestrue);
        m_Gesture.AddGesture(GESTURE_STR, NULL);

        // create hand generator
        m_eResult = m_Hand.Create( m_Context );
        XnCallbackHandle hHandle;
        if( CheckError( "Create Hand Generator Error" ) )
            return false;
        m_Hand.SetSmoothing(0.5f);
        m_Hand.RegisterHandCallbacks(HandCreate, HandUpdate, HandDestroy, NULL, hHandle);

		return true;
	}

	/* Start to get the data from device */
	bool Start()
	{
		m_eResult = m_Context.StartGeneratingAll();
		return !CheckError( "Start Generating" );
	}

	/* Update / Get new data */
	bool UpdateData()
	{
		// update
		m_eResult = m_Context.WaitNoneUpdateAll();
		if( CheckError( "Update Data" ) )
			return false;

		// get new data
		m_Depth.GetMetaData( m_DepthMD );
		m_Image.GetMetaData( m_ImageMD );

		return true;
	}



public:
	xn::DepthMetaData		m_DepthMD;
	xn::ImageMetaData		m_ImageMD;

    bool isHandOk() const
    {
        return m_HandIsOk;
    }

    XnPoint3D getHandPosition() const
    {
        XnPoint3D wPos;
#if 0
        qDebug() << "Hand[" << nId << "]: " <<
                    wPos.X << "/" << wPos.Y << "/" << wPos.Z;
#endif
        m_Depth.ConvertRealWorldToProjective(1, &m_HandPosition, &wPos);
        return wPos;

    }

private:
    /* Check return status m_eResult.
     * return false if the value is XN_STATUS_OK, true for error */
    bool CheckError( const char* sError )
    {
        if( m_eResult != XN_STATUS_OK )
        {
            cerr << sError << ": " << xnGetStatusString( m_eResult ) << endl;
            return true;
        }
        return false;
    }

    static void XN_CALLBACK_TYPE GestureRecognized(xn::GestureGenerator &generator,
                    const XnChar *strGesture,
                    const XnPoint3D *pIDPostion,
                    const XnPoint3D *pEndPosition,
                    void *pCookie)
    {
        qDebug() << "Start moving";
        generator.RemoveGesture(GESTURE_STR);
        m_Hand.StartTracking(*pEndPosition);
    }

    static void XN_CALLBACK_TYPE HandCreate(xn::HandsGenerator &generator,
            XnUserID nId, const XnPoint3D *pPosition, XnFloat fTime, void *pCookie)
    {
        qDebug() << "New Hand: " << nId << " detected!";
        qDebug() << pPosition->X << "/" << pPosition->Y << "/" << pPosition->Z;
        m_HandIsOk = true;
        m_HandPosition = *pPosition;
    }

    static void XN_CALLBACK_TYPE HandUpdate(xn::HandsGenerator &generator,
        XnUserID nId, const XnPoint3D *pPosition, XnFloat fTime, void *pCookie)
    {
#if 0
        qDebug() << "Hand move: ";
        qDebug() << pPosition->X << "/" << pPosition->Y << "/" << pPosition->Z;
#endif
        m_HandPosition = *pPosition;
    }


    static void XN_CALLBACK_TYPE HandDestroy(xn::HandsGenerator &generator,
        XnUserID nId, XnFloat fTime, void *pCookie)
    {
        qDebug() << "Lost Hand: " << nId;
        m_Gesture.AddGesture(GESTURE_STR, NULL);
        m_HandIsOk = false;
    }
	
private:
	XnStatus			m_eResult;
	xn::Context			m_Context;
	xn::DepthGenerator	m_Depth;
	xn::ImageGenerator	m_Image;

    static xn::HandsGenerator  m_Hand;
    static xn::GestureGenerator m_Gesture;

    static bool m_HandIsOk;

    static XnPoint3D m_HandPosition;

};

bool COpenNI::m_HandIsOk = false;
xn::HandsGenerator COpenNI::m_Hand;
xn::GestureGenerator COpenNI::m_Gesture;
XnPoint3D COpenNI::m_HandPosition;


/* Timer to update image in scene from OpenNI */
class CKinectReader: public QObject
{
public:
	/* Constructor */
	CKinectReader( COpenNI& rOpenNI, QGraphicsScene& rScene )
        : m_OpenNI( rOpenNI ), m_Scene( rScene ), m_pItemHand(NULL)
	{}

	/* Destructor */
	~CKinectReader()
	{
        m_Scene.removeItem( m_pItemImage);
        if(m_pItemHand != NULL)
            m_Scene.removeItem(m_pItemHand);
        m_Scene.removeItem(m_pItemAction);
	}

	/* Start to update Qt Scene from OpenNI device */
    bool Start( int iInterval = 33 )
    //bool Start( int iInterval = 1000 )
	{
		m_OpenNI.Start();

		// add an empty Image to scene
		m_pItemImage = m_Scene.addPixmap( QPixmap() );
		m_pItemImage->setZValue( 1 );


        m_pItemAction = m_Scene.addText("Action: ");
        m_pItemAction->setZValue(2);
        m_pItemAction->setFont(QFont("MS Shell Dlg 2", 30));

        m_pItemHand = m_Scene.addEllipse(20, 20, 15, 15, QPen(), QBrush(QColor::fromRgb(0, 255, 0)));
        m_pItemHand->setZValue(3);

		// update first to get the depth map size
		m_OpenNI.UpdateData();

        startTimer(iInterval);

        m_ActionWidget.show();
		return true;
	}

private:
    void captureAction(XnPoint3D &pos)
    {
        double diffX = pos.X - m_LastHandPos.X;
        double diffY = pos.Y - m_LastHandPos.Y;

        m_LastHandPos.X = pos.X;
        m_LastHandPos.Y = pos.Y;

        if((pos.X == diffX) &&
                (pos.Y == diffY))
            return ;


        if(diffX > D)
        {
            cout << "Right: " << diffX << endl;
            m_Action = "Right";
            m_ActionWidget.setAction(Widget::ACTION_RIGHT);
            return ;
        }

        if(diffX < -D)
        {
            cout << "Left" << endl;
            m_Action = "Left";
            m_ActionWidget.setAction(Widget::ACTION_LEFT);
            return ;
        }

        if(diffY > D)
        {
            cout << "Down" << endl;
            m_Action = "Down";
            m_ActionWidget.setAction(Widget::ACTION_DOWN);
            return ;
        }

        if(diffY < -D)
        {
            cout << "Up" << endl;
            m_Action = "Up";
            m_ActionWidget.setAction(Widget::ACTION_UP);
            return ;
        }
    }

private:
	COpenNI&				m_OpenNI;
	QGraphicsScene&			m_Scene;
    QGraphicsEllipseItem*   m_pItemHand;
	QGraphicsPixmapItem*	m_pItemImage;
    QGraphicsTextItem*      m_pItemAction;
    enum {D = 10};
    QString m_Action;


    XnPoint3D m_LastHandPos;

    Widget m_ActionWidget;


private:
	void timerEvent( QTimerEvent *event )
	{
        event->ignore();
        QApplication::processEvents();
		// Read OpenNI data
		m_OpenNI.UpdateData();

		// Read Image
		{
            // Update Image data
			m_pItemImage->setPixmap( QPixmap::fromImage( QImage( m_OpenNI.m_ImageMD.Data(), m_OpenNI.m_ImageMD.XRes(), m_OpenNI.m_ImageMD.YRes(), QImage::Format_RGB888 ) ) );
		}

        if(!m_OpenNI.isHandOk())
        {
            m_pItemHand->setZValue(-1);
            m_LastHandPos.X = 0;
            m_LastHandPos.Y = 0;
            m_LastHandPos.Z = 0;
            m_Action = "Stop";
            m_ActionWidget.setAction(Widget::ACTION_STOP);
        }
        else
        {
            m_pItemHand->setZValue(3);
            XnPoint3D pos = m_OpenNI.getHandPosition();
            qDebug() << "Hand Point: " << pos.X << pos.Y;
            m_pItemHand->setPos(pos.X, pos.Y);
            captureAction(pos);
        }

        m_pItemAction->setPlainText("Action: " + m_Action);
    }
};

/* Main function */
int main( int argc, char** argv )
{
	// initial OpenNI
	COpenNI mOpenNI;
    //bool bStatus = true;
	if( !mOpenNI.Initial() )
		return 1;

	// Qt Application
	QApplication App( argc, argv );
	QGraphicsScene  qScene;

	// Qt View
	QGraphicsView qView( &qScene );
	qView.resize( 650, 540 );
	qView.show();

	// Timer to update image
	CKinectReader KReader( mOpenNI, qScene );

	// start!
	KReader.Start();
	return App.exec();
}
